package com.codinbros.session001.controller;

import java.awt.Point;
import java.util.ArrayList;

import processing.core.PVector;

import com.codinbros.session001.interfaces.Citizen;

public class FlockingBehaviour {
	private ArrayList<Citizen> flock;

	public FlockingBehaviour() {
		flock = new ArrayList<>();
	}

	public Citizen flock(Citizen citizen) {
		PVector sep = separate(citizen); // Separation
		PVector ali = align(citizen); // Alignment
		PVector coh = cohesion(citizen); // Cohesion
		// Arbitrarily weight these forces
		sep.mult(1.5f);
		ali.mult(1.0f);
		coh.mult(1.0f);
		// Add the force vectors to velocity
		PVector vel = citizen.getVelocity();
		vel.add(sep);
		vel.add(ali);
		vel.add(coh);
		vel.limit(citizen.getMaxSpeed());
		citizen.setVelocity(vel);
		return citizen;
	}

	// A method that calculates a steering vector towards a target
	// Takes a second argument, if true, it slows down as it approaches the
	// target
	private PVector steer(PVector target, boolean slowdown, Citizen individual) {
		PVector steer; // The steering vector
		// A vector pointing from the location to the target
		PVector desired = PVector.sub(target, individual.getVelocity());
		float d = desired.mag(); // Distance from the target is the magnitude of
								 // the vector
		// If the distance is greater than 0, calc steering (otherwise return
		// zero vector)
		if (d > 0) {
			// Normalize desired
			desired.normalize();
			// Two options for desired vector magnitude (1 -- based on distance,
			// 2 -- maxspeed)
			// This damping is somewhat arbitrary/5
			if ((slowdown) && (d < 100.0))
				desired.mult((float) (individual.getMaxSpeed() * (d / 100)));
			else
				desired.mult(individual.getMaxSpeed());
			// Steering = Desired minus Velocity
			steer = PVector.sub(desired, individual.getVelocity());

			// Limit to maximum steering force
			steer.limit(individual.getMaxForce());
		} else {
			steer = new PVector(0, 0);
		}
		return steer;
	}

	// Separation
	// Method checks for nearby flock members and steers away
	private PVector separate(Citizen individual) {
		double desiredSeparation = 20.0;
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		// For every boid in the system, check if it's too close
		for (Citizen citizen : flock) {
			float distance = (float) individual.getPostition().dist(citizen.getPostition());
			if ((distance > 0) && (distance < desiredSeparation)) {
				PVector diff = PVector.sub(individual.getPostition(),
						citizen.getPostition());
				diff.normalize();
				diff.div(distance); // Weight by distance
				steer.add(diff);
				count++; // Keep track of how many
			}
		}

		// Average -- divide by how many
		if (count > 0) {
			steer.div((float) count);
		}

		// As long as the vector is greater than 0
		if (steer.mag() > 0) {
			// Implement Reynolds: Steering = Desired - Velocity
			steer.normalize();
			steer.mult(individual.getMaxSpeed());
			steer.sub(individual.getVelocity());
			steer.limit(individual.getMaxForce());
		}
		return steer;
	}

	private PVector align(Citizen individual) {
		float neighbordist = 25.0f;
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		for (Citizen citizen : flock) {
			float distance = (float) individual.getPostition().dist(
					citizen.getPostition());
			if ((distance > 0) && (distance < neighbordist)) {
				steer.add(citizen.getVelocity());
				count++;
			}
		}
		if (count > 0) {
			steer.div((float) count);
		}

		// As long as the vector is greater than 0
		if (steer.mag() > 0) {
			// Implement Reynolds: Steering = Desired - Velocity
			steer.normalize();
			steer.mult(individual.getMaxSpeed());
			steer.sub(individual.getVelocity());
			steer.limit(individual.getMaxForce());
		}
		return steer;
	}

	private PVector cohesion(Citizen individual) {
		float neighbordist = 25.0f;
		// Start with empty vector to accumulate all locations
		PVector sum = new PVector(0, 0);
		PVector steer = new PVector(0, 0, 0);
		int count = 0;
		for (Citizen citizen : flock) {
			float distance = (float) individual.getPostition().dist(
					citizen.getPostition());
			if ((distance > 0) && (distance < neighbordist)) {
				steer.add(citizen.getPostition());
				count++;
			}
		}
		if (count > 0) {
			sum.div((float) count);
			return steer(sum, false, individual); // Steer towards the location
		}
		return sum;
	}

	public void addFlock(ArrayList<Citizen> flock) {
		this.flock = flock;
	}

	public ArrayList<Citizen> getFlock() {
		return flock;
	}
}
